#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include "aimage.h"
#include "aimage_utils.h"

void AIMAGE::setpixel( int x, int y, APIXEL p )
{
	assert( m_image != NULL );
	if ( x < 0 || x >= m_width || y < 0 || y >= m_height )
	{
		return;
	}

	m_image[ y ][ x ] = p; 
}

void AIMAGE::getpixel( int x, int y, APIXEL &p ) const
{
	assert( x >= 0 && x < m_width );
	assert( y >= 0 && y < m_height);
	assert( m_image != NULL );

	p = m_image[ y ][ x ];
}

void AIMAGE::raisepixel( int x, int y, const APIXEL &p ) 
{
	if ( x < 0 || x >= m_width || y < 0 || y >= m_height )
	{
		return;
	}

	int corig, cadd;

	APIXEL &porig = m_image[ y ][ x ];

	corig = ( int ) porig.r;
	cadd = ( int ) p.r;
	corig += cadd;
	if ( corig > FULL_INT )
	{
		corig = FULL_INT;
	}
	porig.r = corig;

	corig = ( int ) porig.g;
	cadd = ( int ) p.g;
	corig += cadd;
	if ( corig > FULL_INT )
	{
		corig = FULL_INT;
	}
	porig.g = corig;

	corig = ( int ) porig.b;
	cadd = ( int ) p.b;
	corig += cadd;
	if ( corig > FULL_INT )
	{
		corig = FULL_INT;
	}
	porig.b = corig;
}

void AIMAGE::line( float x0, float y0, float x1, float y1, APIXEL p, bool smooth )
{
//	assert( x0 >= 0.0f && x1 >= 0.0f );
//	assert( y0 >= 0.0f && y1 >= 0.0f );
//	assert( x0 < (float) ( m_width + 1)); 
//	assert( x1 < (float) ( m_width + 1)); 
//	assert( y0 < (float) ( m_height + 1)); 
//	assert( y1 < (float) ( m_height + 1)); 

	float x_diff;
	float y_diff;

	x_diff = x1 - x0;
	y_diff = y1 - y0;

	float d;
	d = sqrtf( x_diff * x_diff + y_diff * y_diff );
	float alpha_per_t = d / 1.4f;

	float t = 0.0f;
	float tmax = 1.0f;
	float xnext_edge;
	float ynext_edge;
	float xone_pixel;
	float yone_pixel;

	if ( x_diff > 0.0f )
	{
		xnext_edge = -(((float)(-((int) (-x0) ))) - x0) / x_diff;
		xone_pixel = 1.0 / x_diff;
	}
	else if ( x_diff < 0.0f )
	{
		xnext_edge = (((float)((int) x0 )) - x0) / x_diff;
		xone_pixel = -1.0 / x_diff;
	}
	else
	{
		xnext_edge = 1.0f;
		xone_pixel = 1e10;
	}

	if ( y_diff > 0.0f )
	{
		ynext_edge = -(((float)(-((int) (-y0) ))) - y0) / y_diff;
		yone_pixel = 1.0 / y_diff;
	}
	else if ( y_diff < 0.0f )
	{
		ynext_edge = (((float)((int) y0 )) - y0) / y_diff;
		yone_pixel = -1.0 / y_diff;
	}
	else
	{
		ynext_edge = 1.0f;
		yone_pixel = 1e10;
	}

	while ( t < 1.0f )
	{
		float xf0,yf0;
		float xf1,yf1;
		int x,y;
		float tinc;

		xf0 = x0 + x_diff * t;
		yf0 = y0 + y_diff * t;

		if ( xnext_edge < ynext_edge )
		{
			t += xnext_edge;
			tinc = xnext_edge;
			ynext_edge -= xnext_edge;
			xnext_edge = xone_pixel;
		}
		else
		{
			t += ynext_edge;
			tinc = ynext_edge;
			xnext_edge -= ynext_edge;
			ynext_edge = yone_pixel;
		}
		xf1 = x0 + x_diff * t;
		yf1 = y0 + y_diff * t;
		x = ((int) (xf0 + xf1))/2;
		y = ((int) (yf0 + yf1))/2;


		float alpha = tinc * alpha_per_t;
		if ( alpha > 1.0f ) alpha = 1.0f;
		unsigned int alphaint;
		unsigned int onemalphaint;
		alphaint = (alpha * 256);
		onemalphaint = 256 - alphaint;

		//printf("%g %g -> %g %g (%d %d) %g %g %g %d %d\n", xf0, yf0, xf1, yf1, x, y, xnext_edge, ynext_edge, alpha, alphaint, onemalphaint );

		if ( x < 0 || y < 0 || x >= m_width || y>= m_height )
		{
			// Do nothing
		} 
		else if ( smooth )
		{
			APIXEL curp;

			getpixel( x, y, curp );

			curp.r = (((unsigned int ) curp.r) * onemalphaint +
				((unsigned int) p.r ) * alphaint ) / 256;
			curp.g = (((unsigned int ) curp.g) * onemalphaint +
				((unsigned int) p.g ) * alphaint ) / 256;
			curp.b = (((unsigned int ) curp.b) * onemalphaint +
				((unsigned int) p.b ) * alphaint ) / 256;

			setpixel( x, y, curp );
		}
		else
		{
			setpixel( x, y, p );
		}
	}
}

void AIMAGE::circle( float xc, float yc, float rad, APIXEL p, bool smooth )
{
	float r;
	float r_add;
	float last_x, last_y;

	if ( rad < 1.0f )
	{
		setpixel( xc, yc, p );
		return;
	}

	r_add = M_PI * 2 / rad / 3;

	for ( r = 0; r <= M_PI * 2 + r_add ; r+= r_add )
	{
		float x,y;
		x = cos( r ) * rad + xc;
		y = sin( r ) * rad + yc;

		if ( r != 0 )
		{
			line( x, y, last_x, last_y, p, smooth );
		}

		last_x = x;
		last_y = y;

//		if ( x >=0 && y>=0 && x< m_width && y < m_height )
//		{
//			setpixel( x, y, p );
//		}
	}
}


void AIMAGE::do_edge( const AIMAGE &src )
{
	int width, height;
	src.get_dim( width, height );
	APIXEL black( 0.0f, 0.0f, 0.0f );

	assert( width == m_width );
	assert( height == m_height );

	AIU::cls( *this, FULL_INT, 0, 0);
	int x;
	
	for ( x = 1; x < m_width-1; ++x )
	{
		int y;
		for ( y = 1; y < m_height-1; ++y )
		{
			int xf;

			AVECTOR xv(0), yv(0);
			AVECTOR subgrid[3][3];
			AVECTOR sum;
			float xmag, ymag, mag;

			for ( xf = -1; xf <=1; ++xf )
			{
				int yf;
				for ( yf = -1; yf <=1; ++yf )
				{
					APIXEL p;
					int xs,ys;

					xs = x + xf;
					ys = y + yf;
					//printf("%d %d %d %d\n", x, y, xs, ys );
					src.getpixel( xs, ys, p );
					subgrid[ yf + 1 ][ xf + 1 ] = p;
				}
			}

			xv = 	subgrid[ 0 ][ 0 ] * -1.0f +
				subgrid[ 0 ][ 1 ] * -2.0f +
				subgrid[ 0 ][ 2 ] * -1.0f +
				subgrid[ 2 ][ 0 ] * 1.0f +
				subgrid[ 2 ][ 1 ] * 2.0f +
				subgrid[ 2 ][ 2 ] * 1.0f;

			yv = 	subgrid[ 0 ][ 0 ] * -1.0f +
				subgrid[ 1 ][ 0 ] * -2.0f +
				subgrid[ 2 ][ 0 ] * -1.0f +
				subgrid[ 0 ][ 2 ] * 1.0f +
				subgrid[ 1 ][ 2 ] * 2.0f +
				subgrid[ 2 ][ 2 ] * 1.0f;

			xmag = xv.mag();
			ymag = yv.mag();

			mag = sqrt( xmag * xmag + ymag * ymag );

			if ( mag >= 1.0 ) mag = 1.0;

			//printf("%d %d %g\n", x, y, mag );

			APIXEL pd( mag, mag, mag );

			setpixel( x, y, pd ); 
		}
	}
}

void AIMAGE::get_dim( int &width, int &height ) const
{
	width = m_width;
	height = m_height;
}

void AIMAGE::line_callback( float x0, float y0, float x1, float y1, ALINECALLBACK &cb )
{
	if ( x0 < 0.0f && x1 < 0.0f )
	{
		return;
	}
	if ( y0 < 0.0f && y1 < 0.0f )
	{
		return;
	}
	if ( x0 >= m_width && x1 >= m_width )
	{
		return;
	} 
	if ( y0 >= m_height && y1 >= m_height )
	{
		return;
	}
	//assert( x0 >= 0.0f && x1 >= 0.0f );
	//assert( y0 >= 0.0f && y1 >= 0.0f );
	//assert( x0 < (float) ( m_width + 1)); 
	//assert( x1 < (float) ( m_width + 1)); 
	//assert( y0 < (float) ( m_height + 1)); 
	//assert( y1 < (float) ( m_height + 1)); 

	float x_diff;
	float y_diff;

	x_diff = x1 - x0;
	y_diff = y1 - y0;

	float d;
	d = sqrtf( x_diff * x_diff + y_diff * y_diff );
	float alpha_per_t = d / 1.4f;

	float t = 0.0f;
	float tmax = 1.0f;
	float xnext_edge;
	float ynext_edge;
	float xone_pixel;
	float yone_pixel;

	if ( x_diff > 0.0f )
	{
		xnext_edge = -(((float)(-((int) (-x0) ))) - x0) / x_diff;
		xone_pixel = 1.0 / x_diff;
	}
	else if ( x_diff < 0.0f )
	{
		xnext_edge = (((float)((int) x0 )) - x0) / x_diff;
		xone_pixel = -1.0 / x_diff;
	}
	else
	{
		xnext_edge = 1.0f;
		xone_pixel = 1e10;
	}

	if ( y_diff > 0.0f )
	{
		ynext_edge = -(((float)(-((int) (-y0) ))) - y0) / y_diff;
		yone_pixel = 1.0 / y_diff;
	}
	else if ( y_diff < 0.0f )
	{
		ynext_edge = (((float)((int) y0 )) - y0) / y_diff;
		yone_pixel = -1.0 / y_diff;
	}
	else
	{
		ynext_edge = 1.0f;
		yone_pixel = 1e10;
	}

	while ( t < 1.0f )
	{
		float xf0,yf0;
		float xf1,yf1;
		int x,y;
		float tinc;

		xf0 = x0 + x_diff * t;
		yf0 = y0 + y_diff * t;

		if ( xnext_edge < ynext_edge )
		{
			t += xnext_edge;
			tinc = xnext_edge;
			ynext_edge -= xnext_edge;
			xnext_edge = xone_pixel;
		}
		else
		{
			t += ynext_edge;
			tinc = ynext_edge;
			xnext_edge -= ynext_edge;
			ynext_edge = yone_pixel;
		}
		xf1 = x0 + x_diff * t;
		yf1 = y0 + y_diff * t;
		x = ((int) (xf0 + xf1))/2;
		y = ((int) (yf0 + yf1))/2;

		float alpha = tinc * alpha_per_t;
		if ( alpha > 1.0f ) alpha = 1.0f;

		APIXEL curp;

		if ( x >= 0 && y >= 0 && x < m_width && y < m_height ) 
			getpixel( x, y, curp );

		cb.setpixel( x, y, alpha, curp );
	}
}

void AIMAGE::blueshift( int b ) 
{
	if ( b > 0 )
	{
		int y;
		for ( y = m_height - 1; y >=b; --y )
		{
			int x;
			int ys = y - b;
			for ( x = 0; x < m_width; ++x )
			{
				m_image[ y ][ x ].b = m_image[ ys ][x ].b; 
			}
		}
	}
}

void AIMAGE::redshift( int r ) 
{
	if ( r > 0 )
	{
		int y;
		for ( y = m_height - 1; y >=r; --y )
		{
			int x;
			int ys = y - r;
			for ( x = 0; x < m_width; ++x )
			{
				m_image[ y ][ x ].r = m_image[ ys ][x ].r; 
			}
		}
	}
	else if ( r < 0 )
	{
		int y;
		for ( y = 0; y < m_height + r; y++ )
		{
			int x;
			int ys = y - r;
			for ( x = 0; x < m_width; ++x )
			{
				m_image[ y ][ x ].r = m_image[ ys ][x ].r; 
			}
		}
	}
}

void AIMAGE::floodfill( int x, int y, AFLOOD_DATA &data, int count ) 
{
	assert( x >= 0 && x < m_width && y >= 0 && y < m_height );

	assert( count < 5000 );
	count++;

	APIXEL &curp = m_image[ y ][ x ];

	if ( data.flood_test( curp ))
	{
		return;
	}

	data.flood_update( x, y, curp );

	int x1, x2, x3;

	for ( x1 = x + 1; x1 < m_width; ++x1 )
	{
		APIXEL &curp = m_image[ y ][ x1 ];
		if ( data.flood_test( curp ))
		{
			break;
		}
		data.flood_update( x1, y, curp );
	}

	for ( x2 = x - 1; x2 >= 0; --x2 )
	{
		APIXEL &curp = m_image[ y ][ x2 ];
		if ( data.flood_test( curp ))
		{
			break;
		}
		data.flood_update( x2, y, curp );
	}

	if ( data.debug_xonly )
	{
		return;
	}

	if ( y < m_height-1 )
	{
		for ( x3 = x2 + 1; x3 < x1; ++x3 )
		{
			APIXEL &curp= m_image[ y + 1 ][ x3 ];
			if ( !data.flood_test( curp ) )
			{
				floodfill( x3, y+1, data, count ); 	
			}
		}
	}

	if ( y > 0 )
	{
		for ( x3 = x2 + 1; x3 < x1; ++x3 )
		{
			APIXEL &curp= m_image[ y - 1 ][ x3 ];
			if ( !data.flood_test( curp ) )
			{
				floodfill( x3, y-1, data, count ); 	
			}
		}
	}

	return;

	if ( x < data.xl )
	{
		data.xl = x;
	}
	if ( y < data.yl )
	{
		data.yl = y;
	}
	if ( x > data.xh )
	{
		data.xh = x;
	}
	if ( y > data.yh )
	{
		data.yh = y;
	}

	if ( y > 0 )
	{
		floodfill( x, y-1, data ); 
	}
	if ( y < m_height-1 )
	{
		floodfill( x, y+1, data ); 
	}
	if ( x > 0 )
	{
		floodfill( x-1, y, data ); 
	}
	if ( x < m_width-1 )
	{
		floodfill( x+1, y, data ); 
	}
}

APIXEL blend_colors( APIXEL c0, APIXEL c1 )
{
	int r,g,b;

	r = (int) c0.r * (int) c1.r;
	g = (int) c0.g * (int) c1.g;
	b = (int) c0.b * (int) c1.b;

	r /= FULL_INT;
	g /= FULL_INT;
	b /= FULL_INT;

	APIXEL rval;
	rval.r = r;
	rval.g = g;
	rval.b = b;
	return rval;
}

void AIMAGE::rotate_and_add( 
	AIMAGE &alpha,
	const AIMAGE &frame,
	ASTAR_TRANSFORM t )
{
	int y;

	const float xc = 5184 / 2;
   	const float yc = 3456 / 2;

	APIXEL p;
	APIXEL alphap;

	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			float xs, ys;
			float xd, yd;
			int xdi, ydi;

			xs = x - xc; 
			ys = y - yc;

			xd = cosf( t.angle ) * xs - sinf( t.angle ) * ys + t.xo;
			yd = sinf( t.angle ) * xs + cosf( t.angle ) * ys + t.yo;
 
			xdi = xd;
			ydi = yd;

			xdi += xc;
			ydi += yc;

			if ( xdi < 0 || xdi >= m_width || ydi < 0 || ydi >= m_height )
			{
				continue;
			}

			frame.getpixel( xdi, ydi, p );
			raisepixel( x, y, p );
			alpha.getpixel( x, y, alphap );
			alphap.r++; 
			alphap.g++; 
			alphap.b++; 
			alpha.setpixel( x, y, alphap );
		}
	}
}

void AIMAGE::adjust_by_alpha( const AIMAGE &alpha )
{
	int y;
	APIXEL p;
	APIXEL alphap;

	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			getpixel( x, y, p );
			alpha.getpixel( x, y, alphap );

			int r,g,b,alpha;

			r = p.r;
			g = p.g;
			b = p.b;
			alpha = alphap.r;

			if ( alpha > 0 )
			{
				r = r * 256 / alpha;
				g = g * 256 / alpha;
				b = b * 256 / alpha;
			}
			else
			{
				r = 0;
				g = 0;
				b = 0;
			}
			p.r = r;
			p.g = g;
			p.b = b;

			setpixel( x, y, p );
		}
	}
}

AHSV pixel_to_hsv( APIXEL src )
{
	float r, g, b;

	r = (( float ) src.r) / ((float) FULL_INT ) ;
	g = (( float ) src.g) / ((float) FULL_INT ) ;
	b = (( float ) src.b) / ((float) FULL_INT ) ;

	// RGB are from 0..1, H is from 0..360, SV from 0..1
	float maxC = b;
	if (maxC < g) maxC = g;
	if (maxC < r) maxC = r;
	float minC = b;
	if (minC > g) minC = g;
	if (minC > r) minC = r;

	float delta = maxC - minC;


	float V = maxC;
	float S = 0;
	float H = 0;

	if (delta == 0)
	{
		H = 0;
		S = 0;
	}
	else
	{
		S = delta / maxC;
		float dR = 60*(maxC - r)/delta + 180;
		float dG = 60*(maxC - g)/delta + 180;
		float dB = 60*(maxC - b)/delta + 180;
		if (r == maxC)
			H = dB - dG;
		else if (g == maxC)
			H = 120 + dR - dB;
		else
			H = 240 + dG - dR;
	}

	if (H<0)
		H+=360;
	if (H>=360)
		H-=360;

	AHSV rval;
	rval.h = H;
	rval.s = S;
	rval.v = V;
	return rval;
} 

APIXEL hsv_to_pixel( AHSV src )
{
	float H,S,V;
	float R,G,B;

	H = src.h;
	S = src.s;
	V = src.v;

	if( V == 0 )
	{ 
		R = 0; G = 0; B = 0; 
	}
	else if( S == 0 )
	{
  		R = V;
  		G = V;
  		B = V;
	}
	else
	{
  		const float hf = H / 60.0;
  		const int    i  = (int) floor( hf );
  		const float f  = hf - i;
  		const float pv  = V * ( 1 - S );
  		const float qv  = V * ( 1 - S * f );
  		const float tv  = V * ( 1 - S * ( 1 - f ) );
  		switch( i )
		{
			case 0:
      			R = V;
      			G = tv;
      			B = pv;
      			break;
			case 1:
				R = qv;
				G = V;
				B = pv;
				break;
			case 2:
				R = pv;
				G = V;
				B = tv;
				break; 
			case 3:
				R = pv;
				G = qv;
				B = V;
				break;
			case 4:
				R = tv;
				G = pv;
				B = V;
				break;
			case 5:
				R = V;
				G = pv;
				B = qv;
				break;     
		}
	}
	APIXEL rval;
	rval.r = (int) (R * ( float ) FULL_INT );
	rval.g = (int) (G * ( float ) FULL_INT );
	rval.b = (int) (B * ( float ) FULL_INT );
	return rval;
}

void AIMAGE::diff( const AIMAGE &other )
{
	int y;
	APIXEL p;
	APIXEL otherp;

	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			getpixel( x, y, p );
			other.getpixel( x, y, otherp );

			int r,g,b;

			r = ((int ) p.r) - ((int) otherp.r );
			g = ((int ) p.g) - ((int) otherp.g );
			b = ((int ) p.b) - ((int) otherp.b );

			if ( r < 0 ) r = -r;
			if ( g < 0 ) g = -g;
			if ( b < 0 ) b = -b;

			r *= 20;
			g *= 20;
			b *= 20;

			if ( r > FULL_INT ) r = FULL_INT;
			if ( g > FULL_INT ) g = FULL_INT;
			if ( b > FULL_INT ) b = FULL_INT;

			p.r = r;
			p.g = otherp.g;
			//p.b = b;

			setpixel( x, y, p );
		}
	}
}

void AIMAGE::add( const AIMAGE &other )
{
	int y;
	APIXEL p;
	APIXEL otherp;

	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			getpixel( x, y, p );
			other.getpixel( x, y, otherp );

			int r,g,b;

			r = ((int ) p.r) + ((int) otherp.r );
			g = ((int ) p.g) + ((int) otherp.g );
			b = ((int ) p.b) + ((int) otherp.b );

			if ( r > FULL_INT ) r = FULL_INT;
			if ( g > FULL_INT ) g = FULL_INT;
			if ( b > FULL_INT ) b = FULL_INT;

			p.r = r;
			p.g = g;
			p.b = b;

			setpixel( x, y, p );
		}
	}
}

#ifdef GONE
void AIMAGE::gamma( float gamma)
{
	int pre_gamma[ 256 ];
	gamma = 1.0 / gamma;
	int i;

	for ( i = 0; i < 256; ++i )
	{
		float f = (( float) i ) / 256.0f;
		float fg;

		fg = exp( log( f ) * gamma );

		pre_gamma[ i ] = (int) (fg * 256.0f);
	}

	int y;
	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			APIXEL p, newp;
			getpixel( x, y, p );

			newp.r = pre_gamma[ p.r >> 8 ] << 8;
			newp.g = pre_gamma[ p.g >> 8 ] << 8;
			newp.b = pre_gamma[ p.b >> 8 ] << 8;

			setpixel( x, y, newp );
		}
	}
}
#endif

#ifdef GONE
void AIMAGE::blend( const AIMAGE &other )
{
	int y;
	APIXEL p;
	APIXEL otherp;

	for ( y = 0; y < m_height; ++y )
	{
		int x;
		for ( x = 0; x < m_width; ++x )
		{
			getpixel( x, y, p );
			other.getpixel( x, y, otherp );

			int r,g,b;

			r = ((int ) p.r) + ((int) otherp.r );
			g = ((int ) p.g) + ((int) otherp.g );
			b = ((int ) p.b) + ((int) otherp.b );

			r/=2;
			g/=2;
			b/=2;

			if ( r > FULL_INT ) r = FULL_INT;
			if ( g > FULL_INT ) g = FULL_INT;
			if ( b > FULL_INT ) b = FULL_INT;

			p.r = r;
			p.g = g;
			p.b = b;

			setpixel( x, y, p );
		}
	}
}
#endif


void APIXEL::diff( const APIXEL &pa, const APIXEL &pb )
{
	int ra, rb, ga, gb, ba, bb;
	int rl,gl,bl;

	ra = pa.r;
	rb = pb.r;
	ga = pa.g;
	gb = pb.g;
	ba = pa.b;
	bb = pb.b;

	rl = ra - rb;
	gl = ga - gb;
	bl = ba - bb;

	if ( rl < 0 ) rl = - rl;
	if ( gl < 0 ) gl = - gl;
	if ( bl < 0 ) bl = - bl;

	r = rl;
	g = gl;
	b = bl;
}

void APIXEL::addto( const APIXEL &pa )
{
	int ra, rb, ga, gb, ba, bb;
	
	ra = pa.r;
	rb = r;
	ga = pa.g;
	gb = g;
	ba = pa.b;
	bb = b;

	int rl,gl,bl;

	rl = ra + rb;
	gl = ga + gb;
	bl = ba + bb;

	if ( rl >= FULL_INT ) rl = FULL_INT-1;
	if ( gl >= FULL_INT ) gl = FULL_INT-1;
	if ( bl >= FULL_INT ) bl = FULL_INT-1;

	r = rl;
	g = gl;
	b = bl;

}

